#ifndef PBVP_VOL_MOMENTUM_H
#define PBVP_VOL_MOMENTUM_H

#include <math.h>
#include <stdio.h>
#include <iostream>
#include <algorithm>

#include "../math/nr.h"
#include "../math/nrutil.h"

namespace PBVP {
	/// class for the three principal Inertias from a moment of inertia matrix
	class Inertia 
	{
	public:
		/// Default constructor
		Inertia() {
			centroid[0] = centroid[1] = centroid[2] = 0.0f;
			I1 = I2 = I3 = 0.0f;
		}

		/// construct the Inertia from a covariant matrix
		/**
		* @param matrix: is a 3x3 covariant matrix with index from 1 to 3
		* @param vol: volume of the shape
		* 
		* @note jacobi and eigstr are from numerical recipe (NR)
		*/
		void construct(float **mtx, float vol = 1.0f) {
			float *eigenVal = fvector(1, 3);
			float **eigenVec = matrix(1, 3, 1, 3);
			int nrot;
			// compute eigen values
			jacobi(mtx, 3, eigenVal, eigenVec, &nrot);
			// sort eigen values
			eigsrt(eigenVal, eigenVec, 3);

			I1 = eigenVal[1] / vol;
			I2 = eigenVal[2] / vol;
			I3 = eigenVal[3] / vol; 

			free_vector(eigenVal, 1, 3);
			free_matrix(eigenVec, 1, 3, 1, 3);
		}

		friend float simInertia(const Inertia & inert1, const Inertia& inert2)
		{
			const float small = 1.0e-5f;
			// all principle values should be positive
			float sim1 = 1 - fabs(inert1.I1 - inert2.I1) / std::max(small, std::max(inert1.I1, inert2.I1));
			float sim2 = 1 - fabs(inert1.I2 - inert2.I2) / std::max(small, std::max(inert1.I2, inert2.I2));
			float sim3 = 1 - fabs(inert1.I3 - inert2.I3) / std::max(small, std::max(inert1.I3, inert2.I3));

			//return 0.5f*sim1 + 0.25f*sim2 + 0.25f*sim3;
			return (sim1 + sim2 + sim3) / 3;
		}

		/// Centroid of a volume
		float centroid[3];
		/// three principal Inertias I1 > I2 > I3
		float I1, I2, I3;
	};

	/// Multipole of potential distribution
	class Multipole
	{
	public:
		/// Default constructor
		Multipole() {
			centroid[0] = centroid[1] = centroid[2] = 0.0f;
			dipole[0] = dipole[1] = dipole[2] = 0.0f;
			Q1 = Q2 = Q3;
		}

		friend float simMultipole(const Multipole& pole1, const Multipole& pole2)
		{
			float di_len1 = (float)sqrt(pole1.dipole[0]*pole1.dipole[0] +
				pole1.dipole[1]*pole1.dipole[1] + pole1.dipole[2]*pole1.dipole[2]);
			float di_len2 = (float)sqrt(pole2.dipole[0]*pole2.dipole[0] +
				pole2.dipole[1]*pole2.dipole[1] + pole2.dipole[2]*pole2.dipole[2]);

			// use 1.0f as the threshold for dipole length
			float sim_di = 1 - fabs(di_len1 - di_len2) / std::max(1.0f, std::max(di_len1, di_len2));
			// use 100.0f as the threshold for multipole 
			float sim_q1 = 1 - fabs(pole1.Q1-pole2.Q1)/std::max(100.0f, (float)std::max(fabs(pole1.Q1), fabs(pole2.Q1)));
			float sim_q2 = 1 - fabs(pole1.Q2-pole2.Q2)/std::max(100.0f, (float)std::max(fabs(pole1.Q2), fabs(pole2.Q2)));		
			float sim_q3 = 1 - fabs(pole1.Q3-pole2.Q3)/std::max(100.0f, (float)std::max(fabs(pole1.Q3), fabs(pole2.Q3)));
			float sim_q = (sim_q1 + sim_q2 + sim_q3) / 3.0f;

			// printf("di_len1 = %f, di_len2 = %f, sim_di = %f, sim_q = %f\n", di_len1, di_len2, sim_di, sim_q);
			return 0.9f * sim_di + 0.1f * sim_q;

		}

		/// Centroid of the volume
		float centroid[3];
		/// Dipole Moment
		float dipole[3];
		/// principal values of the quadrupole moment
		float Q1, Q2, Q3;
	};

	/**
	* Attributes related to the volume moments
	*/
	class MomtAttrib
	{
	public:
		/// Default constructor
		MomtAttrib () {
			I1 = I2 = I3 = 0.0f;
			Fint = 0.0f;
			Fmin = Fmax = 0.0f;
			Dlen = 0.0f;
			Dang = 0.0f;
			Q1 = Q2 = Q3 = 0.0f;
		}

		float I1, I2, I3;	// inertia
		float Fint;
		float Fmin, Fmax;	// integral of f over the volume
		float Dlen;			// the length of the dipole moment
		float Dang;			// Cos of the angle between the dipole moment and the principal axis
		float Q1, Q2, Q3;	// major components of the quardruple moment

		void print();
	};

	/************************************************************************/
	/* The first and second order moments of a volume                       */
	/************************************************************************/

	class VolMoments
	{
	public:
		VolMoments() {
			vol = 0; 
			Px = Py = Pz = 0;
			Pxx = Pyy = Pzz = Pxy = Pxz = Pyz = 0;
			Fint = 0;
			Fmin = 0; Fmax = 0;
			Fx = 0; Fy = 0; Fz = 0;
			Fxx = Fyy = Fzz = Fxy = Fxz = Fyz = 0;
		}

		/**
		* Get the center of mass of the volume
		*/
		inline void getCOM(float com[3]);

		/**
		* Get the 3x3 covariance matrix 
		*/
		inline void getCovMatrix(float **mtx);

		/**
		* Print out in a nice format
		*/
		inline void print();

		inline void printRaw();

		/**
		* Convert to matchable attributes
		*/
		MomtAttrib toAttributes();

		/**
		* Compute the eigenvalues and eigenvectors of the covariance matrix
		* The eigenvalues are sorted by their values in descending order
		*/
		void principalAxes(float *eigval, float **eigvec);

		VolMoments& operator = (const VolMoments & mom) {
			if (this == &mom)
			{
				return *this;
			}
			vol = mom.vol;
			Px = mom.Px; Py = mom.Py; Pz = mom.Pz;
			Pxx = mom.Pxx; Pyy = mom.Pyy; Pzz = mom.Pzz;
			Pxy = mom.Pxy; Pxz = mom.Pxz; Pyz = mom.Pyz;
			Fint = mom.Fint; 
			Fmin = mom.Fmin; Fmax = mom.Fmax;
			Fx = mom.Fx; Fy = mom.Fy; Fz = mom.Fz;
			Fxx = mom.Fxx; Fyy = mom.Fyy; Fzz = mom.Fzz;
			Fxy = mom.Fxy; Fxz = mom.Fxz; Fyz = mom.Fyz;
			return *this;
		}

		friend VolMoments operator + (const VolMoments & m1, const VolMoments & m2) {
			VolMoments mom;
			mom.vol = m1.vol + m2.vol;
			mom.Px = m1.Px + m2.Px;
			mom.Py = m1.Py + m2.Py;
			mom.Pz = m1.Pz + m2.Pz;
			mom.Pxx = m1.Pxx + m2.Pxx;
			mom.Pyy = m1.Pyy + m2.Pyy;
			mom.Pzz = m1.Pzz + m2.Pzz;
			mom.Pxy = m1.Pxy + m2.Pxy;
			mom.Pxz = m1.Pxz + m2.Pxz;
			mom.Pyz = m1.Pyz + m2.Pyz;
			mom.Fint = m1.Fint + m2.Fint;
			mom.Fx = m1.Fx + m2.Fx;
			mom.Fy = m1.Fy + m2.Fy;
			mom.Fz = m1.Fz + m2.Fz;
			mom.Fmin = FMIN(m1.Fmin, m2.Fmin);
			mom.Fmax = FMAX(m1.Fmax, m2.Fmax);
			mom.Fxx = m1.Fxx + m2.Fxx;
			mom.Fyy = m1.Fyy + m2.Fyy;
			mom.Fzz = m1.Fzz + m2.Fzz;
			mom.Fxy = m1.Fxy + m2.Fxy;
			mom.Fxz = m1.Fxz + m2.Fxz;
			mom.Fyz = m1.Fyz + m2.Fyz;
			return mom;
		}

		friend std::ostream& operator << (std::ostream& out, const VolMoments& mom) {
			out << mom.vol << " " << mom.Px << " " << mom.Py << " " << mom.Pz << " ";
			out << mom.Pxx << " " << mom.Pyy << " " << mom.Pzz << " ";
			out << mom.Pxy << " " << mom.Pxz << " " << mom.Pyz << " ";
			out << mom.Fint << " " << mom.Fmin << " " << mom.Fmax << " ";
			out << mom.Fx << " " << mom.Fy << " " << mom.Fz << " ";
			out << mom.Fxx << " " << mom.Fyy << " " << mom.Fzz << " ";
			out << mom.Fxy << " " << mom.Fxz << " " << mom.Fyz << std::endl;
			return out;
		}

		friend std::istream& operator >> (std::istream& in, VolMoments& mom) {
			in >> mom.vol >> mom.Px >> mom.Py >> mom.Pz;
			in >> mom.Pxx >> mom.Pyy >> mom.Pzz;
			in >> mom.Pxy >> mom.Pxz >> mom.Pyz;
			in >> mom.Fint >> mom.Fmin >> mom.Fmax;
			in >> mom.Fx >> mom.Fy >> mom.Fz;
			in >> mom.Fxx >> mom.Fyy >> mom.Fzz;
			in >> mom.Fxy >> mom.Fxz >> mom.Fyz;
			return in;
		}

		/************************************************************************/
		/* Member variables                                                     */
		/************************************************************************/
		float vol;
		float Px, Py, Pz;
		float Pxx, Pyy, Pzz, Pxy, Pxz, Pyz;
		/* Moments of the potential f*/
		float Fint;			// integral of f over the volume
		float Fmin, Fmax;
		float Fx, Fy, Fz;
		float Fxx, Fyy, Fzz, Fxy, Fxz, Fyz;
	}; 

	void VolMoments::getCOM(float com[3])
	{
		if (vol <= 0)
		{
			com[0] = com[1] = com[2] = 0.0f;
			return;
		}
		com[0] = Px / vol;
		com[1] = Py / vol;
		com[2] = Pz / vol;
	}

	void VolMoments::getCovMatrix(float **mtx)
	{
		int i, j;
		if (vol <= 0)
		{
			for (i = 1; i <= 3; i++)
			{
				for (j = 1; j <= 3; j++)
				{
					mtx[i][j] = 0;
				}
			}
			return;
		}
		float x[3];
		getCOM(x);
		/*
		mtx[1][1] = Pxx - vol*x[0]*x[0];
		mtx[2][2] = Pyy - vol*x[1]*x[1];
		mtx[3][3] = Pzz - vol*x[2]*x[2];
		mtx[1][2] = mtx[2][1] = Pxy - vol*x[0]*x[1];
		mtx[1][3] = mtx[3][1] = Pxz - vol*x[0]*x[2];
		mtx[2][3] = mtx[3][2] = Pyz - vol*x[1]*x[2];
		*/
		mtx[1][1] = Pyy - vol*x[1]*x[1] + Pzz - vol*x[2]*x[2];
		mtx[2][2] = Pxx - vol*x[0]*x[0] + Pzz - vol*x[2]*x[2];
		mtx[3][3] = Pxx - vol*x[0]*x[0] + Pyy - vol*x[1]*x[1];
		mtx[1][2] = mtx[2][1] = -(Pxy - vol*x[0]*x[1]);
		mtx[1][3] = mtx[3][1] = -(Pxz - vol*x[0]*x[2]);
		mtx[2][3] = mtx[3][2] = -(Pyz - vol*x[1]*x[2]);
	}

	void VolMoments::print()
	{
		float ctr[3], **mtx;
		mtx = matrix(1, 3, 1, 3);
		getCOM(ctr);
		getCovMatrix(mtx);
		printf("Volume = %f\n", vol);
		printf("Center: (%f %f %f)\n", ctr[0], ctr[1], ctr[2]);
		for (int i = 1; i <= 3; i++)
		{
			printf("%f\t %f\t %f\n", mtx[i][1], mtx[i][2], mtx[i][3]);
		}

		free_matrix(mtx, 1, 3, 1, 3);
	}

	void VolMoments::printRaw()
	{
		printf("Volume = %f\n", vol);
		printf("Px = %f, Py = %f, Pz = %f\n", Px, Py, Pz);

		printf("%f\t%f\t%f\n", Pxx, Pxy, Pxz);
		printf("%f\t%f\t%f\n", Pxy, Pyy, Pyz);
		printf("%f\t%f\t%f\n", Pxz, Pyz, Pzz);
		printf("Fint = %f\n", Fint);
		printf("Fx = %f, Fy = %f, Fz = %f\n", Fx, Fy, Fz);
		printf("%f\t%f\t%f\n", Fxx, Fxy, Fxz);
		printf("%f\t%f\t%f\n", Fxy, Fyy, Fyz);
		printf("%f\t%f\t%f\n", Fxz, Fyz, Fzz);
	}

}
#endif 
